/** \file    mat2d.h
  * \brief   2D matrix template definition.
  * \date    1998-2001
  * \author  Aloudin (Albert) Akhriev
  * \version $Header: /home/cscom1r4/cvsroot/cs/include/vec2d/mat2d.h,v 1.2 2004/12/23 17:42:00 cscom1r4 Exp $
  */

#ifndef _mat2d_h_
#define _mat2d_h_

#include "vec2d.h"
#include "std_min_max.h"
#include "decl.h"

namespace vec2d {

#define IS_INT(T)	(std::numeric_limits<T>::is_integer)
#define MIN_EPS(T)	(std::numeric_limits<T>::min()/std::numeric_limits<T>::epsilon())

//==========================================================================
// Matrix 2x2.
// 'T' - element type, 'H' - high type (e.g. T = float, H = double, T <= H).
//==========================================================================
template<class T, class H>
struct Mat2D
{
  T m[2][2];

  Mat2D() {}

  Mat2D(T v00, T v01, T v10, T v11)
  {
    m[0][0] = v00;    m[0][1] = v01;
    m[1][0] = v10;    m[1][1] = v11;
  }


  Mat2D(const Vec2D<T,H> & v1, const Vec2D<T,H> & v2) 
  {
    m[0][0] = v1.x * v2.x;                  // vector1 * Transposed(vector2)
    m[0][1] = v1.x * v2.y;
    m[1][0] = v1.y * v2.x;
    m[1][1] = v1.y * v2.y;
  }


  Mat2D(T cs, T sn)
  { 
    assert(!IS_INT(T));

    m[0][0] = cs;   m[0][1] = -sn;          // matrix of rotation
    m[1][0] = sn;   m[1][1] = cs;
  }


  Mat2D(T angle)
  {
    assert(!IS_INT(T));

    T cs = (T)cos(angle);                   // matrix of rotation
    T sn = (T)sin(angle);

    m[0][0] = cs;   m[0][1] = -sn;
    m[1][0] = sn;   m[1][1] = cs;
  }


  template<class P, class Q>
  Mat2D(const Mat2D<P,Q> & A)
  {
    if (IS_INT(T) && !IS_INT(P))
    {
      m[0][0] = (T)(round(A.m[0][0]));
      m[0][1] = (T)(round(A.m[0][1]));
      m[1][0] = (T)(round(A.m[1][0]));
      m[1][1] = (T)(round(A.m[1][1]));
    }
    else
    {
      m[0][0] = (T)(A.m[0][0]);
      m[0][1] = (T)(A.m[0][1]);
      m[1][0] = (T)(A.m[1][0]);
      m[1][1] = (T)(A.m[1][1]);
    }
  }


  template<class P, class Q>
  Mat2D<T,H> & operator=(const Mat2D<P,Q> & A)
  {
    if (IS_INT(T) && !IS_INT(P))
    {
      m[0][0] = (T)(round(A.m[0][0]));
      m[0][1] = (T)(round(A.m[0][1]));
      m[1][0] = (T)(round(A.m[1][0]));
      m[1][1] = (T)(round(A.m[1][1]));
    }
    else
    {
      m[0][0] = (T)(A.m[0][0]);
      m[0][1] = (T)(A.m[0][1]);
      m[1][0] = (T)(A.m[1][0]);
      m[1][1] = (T)(A.m[1][1]);
    }
    return (*this);
  }


  void ZeroInit() 
  { 
    m[0][0] = m[0][1] = m[1][0] = m[1][1] = (T)0; 
  }


  void Unity()
  { 
    m[0][0] = m[1][1] = (T)1;
    m[0][1] = m[1][0] = (T)0;
  }


  T & M(int i, int j)        // M[row,column]
  { 
    assert((i == 0 || i == 1) && (j == 0 || j == 1));

    return (i == 0) ? ((j == 0) ? m[0][0] : m[0][1])
                    : ((j == 0) ? m[1][0] : m[1][1]);
  }


  T M(int i, int j) const    // M[row,column]
  { 
    assert((i == 0 || i == 1) && (j == 0 || j == 1));

    return (i == 0) ? ((j == 0) ? m[0][0] : m[0][1])
                    : ((j == 0) ? m[1][0] : m[1][1]);
  }


  Vec2D<T,H> operator*=(const Vec2D<T,H> & v)
  {
    return Vec2D<T,H>(m[0][0]*v.x + m[0][1]*v.y,
                      m[1][0]*v.x + m[1][1]*v.y);
  }


  Mat2D<T,H> & operator+=(const Mat2D<T,H> & A)
  {
    m[0][0] += A.m[0][0];
    m[0][1] += A.m[0][1];
    m[1][0] += A.m[1][0];
    m[1][1] += A.m[1][1];
    return (*this);
  }


  Mat2D<T,H> & operator-=(const Mat2D<T,H> & A)
  {
    m[0][0] -= A.m[0][0];
    m[0][1] -= A.m[0][1];
    m[1][0] -= A.m[1][0];
    m[1][1] -= A.m[1][1];
    return (*this);
  }


  Mat2D<T,H> & operator*=(const Mat2D<T,H> & A) 
  {
	T R[2][2] = {m[0][0], m[0][1], m[1][0], m[1][1]};
    m[0][0] = R[0][0] * A.m[0][0]  +  R[0][1] * A.m[1][0];
    m[0][1] = R[0][0] * A.m[0][1]  +  R[0][1] * A.m[1][1];
    m[1][0] = R[1][0] * A.m[0][0]  +  R[1][1] * A.m[1][0];
    m[1][1] = R[1][0] * A.m[0][1]  +  R[1][1] * A.m[1][1];
    return (*this);
  }


  Mat2D<T,H> & operator*=(T coef)
  {
    m[0][0] *= coef;  
    m[0][1] *= coef;  
    m[1][0] *= coef;  
    m[1][1] *= coef;
    return (*this);
  }


  Mat2D<T,H> & operator/=(T coef)
  {
    if (IS_INT(T))
    {
      if (coef != 0)
      {
        m[0][0] /= coef;  
        m[0][1] /= coef;  
        m[1][0] /= coef;  
        m[1][1] /= coef;
      }
      else ZeroInit();
    }
    else
    {
      coef = (fabs(coef) > MIN_EPS(T)) ? (T)(1.0/coef) : (T)0.0;
      m[0][0] *= coef;  
      m[0][1] *= coef;  
      m[1][0] *= coef;  
      m[1][1] *= coef;
    }
    return (*this);
  }


  Mat2D<T,H> Transposed() const
  {
    Mat2D<T,H> A;
    A.m[0][0] = m[0][0];
    A.m[0][1] = m[1][0];
    A.m[1][0] = m[0][1];
    A.m[1][1] = m[1][1];
    return A;    
  }


  Vec2D<T,H> Column(int c) const
  {
    assert(c == 0 || c == 1);

    return (c == 0) ? Vec2D<T,H>(m[0][0],
                                 m[1][0]) : Vec2D<T,H>(m[0][1],
                                                       m[1][1]);
  }


  Vec2D<T,H> Row(int r) const
  {
    assert(r == 0 || r == 1);

    return (r == 0) ? Vec2D<T,H>(m[0][0], m[0][1])
                    : Vec2D<T,H>(m[1][0], m[1][1]);
  }


  T GetAbsMax() const
  {
    if (IS_INT(T))
    {
      return (T)(std::max(std::max(abs(m[0][0]), abs(m[0][1])),
                       std::max(abs(m[1][0]), abs(m[1][1]))));
    }
    else
    {
      return (T)(std::max(std::max(fabs(m[0][0]), fabs(m[0][1])),
                       std::max(fabs(m[1][0]), fabs(m[1][1]))));
    }
  }


  H Determinant() const
  {
    return (((H)m[0][0])*((H)m[1][1]) - ((H)m[0][1])*((H)m[1][0]));
  }


  Mat2D<T,H> Reversed() const
  {
    assert(!IS_INT(T));
    Mat2D<T,H> A;
    double det = Determinant();
    double revDet = (fabs(det) > MIN_EPS(T)) ? (T)(1.0/det) : (T)0.0;
    A.m[0][0] = (T)(+m[1][1] * revDet);
    A.m[0][1] = (T)(-m[0][1] * revDet);
    A.m[1][0] = (T)(-m[1][0] * revDet);
    A.m[1][1] = (T)(+m[0][0] * revDet);
    return A;
  }


  H SqrNorm() const
  {
    return (sqr((H)m[0][0]) + sqr((H)m[0][1]) +
            sqr((H)m[1][0]) + sqr((H)m[1][1]));
  }


  H Trace() const
  {
    return ((H)m[0][0] + (H)m[1][1]);
  }


  H Rtace() const
  {
    return ((H)m[0][1] - (H)m[1][0]);
  }

};

////////////////////////////////////////////////////////////////////////////
// Friend Functions.
////////////////////////////////////////////////////////////////////////////

template<class T, class H>
Mat2D<T,H> operator+(const Mat2D<T,H> & A, const Mat2D<T,H> & B)
{
  Mat2D<T,H> R;
  R.m[0][0] = A.m[0][0] + B.m[0][0];
  R.m[0][1] = A.m[0][1] + B.m[0][1];
  R.m[1][0] = A.m[1][0] + B.m[1][0];
  R.m[1][1] = A.m[1][1] + B.m[1][1];
  return R;
}


template<class T, class H>
Mat2D<T,H> operator-(const Mat2D<T,H> & A, const Mat2D<T,H> & B)
{
  Mat2D<T,H> R;
  R.m[0][0] = A.m[0][0] - B.m[0][0];
  R.m[0][1] = A.m[0][1] - B.m[0][1];
  R.m[1][0] = A.m[1][0] - B.m[1][0];
  R.m[1][1] = A.m[1][1] - B.m[1][1];
  return R;
}


template<class T, class H>
Mat2D<T,H> operator*(const Mat2D<T,H> & A, const Mat2D<T,H> & B) 
{
  Mat2D<T,H> R;
  R.m[0][0] = A.m[0][0] * B.m[0][0]  +  A.m[0][1] * B.m[1][0];
  R.m[0][1] = A.m[0][0] * B.m[0][1]  +  A.m[0][1] * B.m[1][1];
  R.m[1][0] = A.m[1][0] * B.m[0][0]  +  A.m[1][1] * B.m[1][0];
  R.m[1][1] = A.m[1][0] * B.m[0][1]  +  A.m[1][1] * B.m[1][1];
  return R;
}


template<class T, class H>
Mat2D<T,H> operator*(const Mat2D<T,H> & A, T coef)
{
  Mat2D<T,H> R;
  R.m[0][0] = A.m[0][0] * coef;  
  R.m[0][1] = A.m[0][1] * coef;  
  R.m[1][0] = A.m[1][0] * coef;  
  R.m[1][1] = A.m[1][1] * coef;
  return R;
}


template<class T, class H>
Mat2D<T,H> operator*(T coef, const Mat2D<T,H> & A)
{
  Mat2D<T,H> R;
  R.m[0][0] = A.m[0][0] * coef;  
  R.m[0][1] = A.m[0][1] * coef;  
  R.m[1][0] = A.m[1][0] * coef;  
  R.m[1][1] = A.m[1][1] * coef;
  return R;
}


template<class T, class H>
Mat2D<T,H> operator/(const Mat2D<T,H> & A, T coef)
{
  Mat2D<T,H> R;
  if (IS_INT(T))
  {
    if (coef != 0)
    {
      R.m[0][0] = A.m[0][0] / coef;  
      R.m[0][1] = A.m[0][1] / coef;  
      R.m[1][0] = A.m[1][0] / coef;  
      R.m[1][1] = A.m[1][1] / coef;
    }
    else R.ZeroInit();
  }
  else
  {
    coef = (fabs(coef) > MIN_EPS(T)) ? (T)(1.0/coef) : (T)0.0;
    R.m[0][0] = A.m[0][0] * coef;  
    R.m[0][1] = A.m[0][1] * coef;  
    R.m[1][0] = A.m[1][0] * coef;  
    R.m[1][1] = A.m[1][1] * coef;
  }
  return R;
}


template<class T, class H>
Vec2D<T,H> operator*(const Mat2D<T,H> & A, const Vec2D<T,H> & v)
{
  return Vec2D<T,H>(A.m[0][0]*v.x + A.m[0][1]*v.y,
                    A.m[1][0]*v.x + A.m[1][1]*v.y);
}


template<class T, class H>
Vec2D<T,H> operator*(const Vec2D<T,H> & v, const Mat2D<T,H> & A)
{
  return Vec2D<T,H>(v.x*A.m[0][0] +
                    v.y*A.m[1][0], v.x*A.m[0][1] + 
                                   v.y*A.m[1][1]);
}

////////////////////////////////////////////////////////////////////////////
// Derived Types.
////////////////////////////////////////////////////////////////////////////

#undef IS_INT
#undef MIN_EPS

}

#endif //_mat2d_h_
